#include "ConstraintDistance.h"

ConstraintDistance::ConstraintDistance(void)
{
}

ConstraintDistance::~ConstraintDistance(void)
{
}

void ConstraintDistance::applyConstraint(Particle& p, float x, float y, float Ks, float Kd)
{
	Vec3f q(x,y,0.0);
	Vec3f vectorD = p.getPosition() - q;
	
	double C  = 0.5*( dot(vectorD, vectorD ) - (9.0/2.0) );	//distance = 3
	double dC = dot( p.getPosition(), p.getVelocity() );

	double l = ( dot(-p.getForce(), p.getPosition()) - ( dot(p.getMass()*p.getVelocity(), p.getVelocity()) ) - (Ks*C) - (Kd*dC) ) 
		/ dot(p.getPosition(), p.getPosition());

	Vec3f f(l*p.getPosition());
	p.addForce(f[0], f[1], f[2]);
}

void ConstraintDistance::applyConstraint2(Particle& p, Particle& q, float Ks, float Kd)
{
	double C = 0.5*( dot(p.getPosition()-q.getPosition(),p.getPosition()-q.getPosition() ) - (9.0/2.0) );
	double dC = dot( p.getPosition(), p.getVelocity());

	double l = ( dot(-p.getForce(), p.getPosition()) - ( dot(p.getMass()*p.getVelocity(), p.getVelocity()) ) - (Ks*C) - (Kd*dC) ) 
		/ dot(p.getPosition(), p.getPosition());

	Vec3f f(l*p.getPosition());
	p.addForce(f[0], f[1], f[2]);
}
